#include "math.h"
#include "Motor.h"

#define AIN1(X)                                                                \
  ((X) ? (DL_GPIO_setPins(TB6612_PORT, TB6612_AIN2_PIN))                       \
       : (DL_GPIO_clearPins(TB6612_PORT, TB6612_AIN2_PIN)))
#define AIN2(X)                                                                \
  ((X) ? (DL_GPIO_setPins(TB6612_PORT, TB6612_AIN1_PIN))                       \
       : (DL_GPIO_clearPins(TB6612_PORT, TB6612_AIN1_PIN)))
#define BIN1(X)                                                                \
  ((X) ? (DL_GPIO_setPins(TB6612_PORT, TB6612_BIN1_PIN))                       \
       : (DL_GPIO_clearPins(TB6612_PORT, TB6612_BIN1_PIN)))
#define BIN2(X)                                                                \
  ((X) ? (DL_GPIO_setPins(TB6612_PORT, TB6612_BIN2_PIN))                       \
       : (DL_GPIO_clearPins(TB6612_PORT, TB6612_BIN2_PIN)))


void Motor_Stop(void) 
{
    AIN1(1);
    AIN2(1);
    BIN1(1);
    BIN2(1);
    DL_TimerG_setCaptureCompareValue(PWM_INST,0,DL_TIMER_CC_0_INDEX);
    DL_TimerG_setCaptureCompareValue(PWM_INST,0,DL_TIMER_CC_1_INDEX);
}

void Set_Pwm(int PWMA, int PWMB) 
{
    AIN1(PWMA > 0);
    AIN2(PWMA < 0);
    BIN1(PWMB < 0);
    BIN2(PWMB > 0);
    //* 限制PWMA和PWMB的值在PWM_MIN和PWM_MAX之间
    PWMA = myabs((PWMA > PWM_MAX) ? PWM_MAX : (PWMA < PWM_MIN ? PWM_MIN : PWMA));
    PWMB = myabs((PWMB > PWM_MAX) ? PWM_MAX : (PWMB < PWM_MIN ? PWM_MIN : PWMB));
    //将PWM值
    DL_TimerG_setCaptureCompareValue(PWM_INST,PWMA,DL_TIMER_CC_0_INDEX);
    DL_TimerG_setCaptureCompareValue(PWM_INST,PWMB,DL_TIMER_CC_1_INDEX);
}

//自定义绝对值函数
int myabs(int a)     
{
  return (a < 0) ? -a : a;
}

